/*
 Name:		factoryTest.ino
 Created:	9/24/2018 11:08:39 AM
 Author:	Artorias
*/
#include "GestureService.h"
#include <M5Stack.h>
#include <Wire.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include "FS.h"
#include "SD.h"
#include "SPI.h"
#include <M5Stack.h>
#include <WiFi.h>


#define SERIAL_BAUD 115200 
#define address 0x1E //0011110b, I2C 7bit address of HMC5883



const char* ssid = "NETGEAR-Guest";
const char* password = "12345678";
const char * udpAddress = "192.168.1.203";
const int udpPort = 3333;
WiFiUDP udp;

GestureService gesture = GestureService();
uint8_t handGesture = 0;







// the setup function runs once when you press reset or power the board
void setup() {


	M5.begin();
	Serial.begin(SERIAL_BAUD);
	Wire.begin();
	
	

	Serial.print("Connecting to ");
	Serial.println(ssid);
	WiFi.begin(ssid, password);
	while (WiFi.status() != WL_CONNECTED) {
		delay(500);
		Serial.print(".");
	}
	Serial.println("");
	Serial.println("WiFi connected");
	Serial.println("IP address: ");
	Serial.println(WiFi.localIP());


	if (!SD.begin(4, SPI, 40000000)) {
		Serial.println("Card Mount Failed");
		return;
	}
	uint64_t cardSize = SD.cardSize() / (1024 * 1024);
	Serial.printf("SD Card Size: %lluMB\n", cardSize);

	M5.Lcd.drawBmpFile(SD, "/Tim.bmp", 0, 0);

	//Put the HMC5883 IC into the correct operating mode
	Wire.beginTransmission(address); //open communication with HMC5883
	Wire.write(0x02); //select mode register
	Wire.write(0x00); //continuous measurement mode
	Wire.endTransmission();

	gesture.init();
	gesture.enterGestureMode();
	delay(300);
	gesture.init();
	gesture.enterGestureMode();

	

}

// the loop function runs over and over again until power down or reset
void loop() {


	int x, y, z; //triple axis data

				 //Tell the HMC5883 where to begin reading data
	Wire.beginTransmission(address);
	Wire.write(0x03); //select register 3, X MSB register
	Wire.endTransmission();


	//Read data from each axis, 2 registers per axis
	Wire.requestFrom(address, 6);
	if (6 <= Wire.available()) {
		x = Wire.read() << 8; //X msb
		x |= Wire.read(); //X lsb
		z = Wire.read() << 8; //Z msb
		z |= Wire.read(); //Z lsb
		y = Wire.read() << 8; //Y msb
		y |= Wire.read(); //Y lsb
	}

	//Print out values of each axis
	Serial.print("x: ");
	Serial.print(x);
	Serial.print("  y: ");
	Serial.print(y);
	Serial.print("  z: ");
	Serial.println(z);

	gesture.enterGestureMode();
	handGesture = gesture.getGesture();

	Serial.println(handGesture);

	if (handGesture) {
		Serial.println(handGesture);
		udp.beginPacket(udpAddress, udpPort);
		udp.printf("HandGesture:%d\n", handGesture);
		udp.endPacket();
	}

	delay(100);
}



